//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================
#include <asm.h>
# *************************************************************************
//  LEAF_ENTRY  FlushICache
.globl  sleep_save_sp
.global C_SYMBOL(pxa_cpu_standby12)
.global C_SYMBOL(pxa_cpu_suspend)
.global C_SYMBOL(pxa_cpu_resume)
.global C_SYMBOL(sleep_save_sp)
.global C_SYMBOL(ReadCLKCFG)
.global C_SYMBOL(WriteCLKCFG)
.global C_SYMBOL(WritePWRMODE)
.global C_SYMBOL(pmu_start_S)
.global C_SYMBOL(ReadCCNT)
.global C_SYMBOL(ReadPMN0)
.global C_SYMBOL(ReadPMN1)
.global C_SYMBOL(ReadPMN2)
.global C_SYMBOL(ReadPMN3)
.global C_SYMBOL(ClearPMNC)
.global C_SYMBOL(set_cpu_mode)
.global C_SYMBOL(get_cpu_mode)
.global C_SYMBOL(GetSP)
.global C_SYMBOL(GetTTB)
.section cpusleep
FlushICache:
#++
# Routine Description:       Called from coreos\...\mdarm.c
#    Flush and invalidate the Instruction Cache
#
# Syntax:
#   void FlushICache(void)#
#
# Arguments:
#   -- none --
#
# Return Value:
#   -- none --
#--

    mcr     p15, 0, r0, c7, c5, 0   // flush the icache & BTB

    mov   pc, lr            // return


# *************************************************************************
//  LEAF_ENTRY  FlushDCache

FlushDCache:

#++
# Routine Description:
#
# The following code is called to flush the dcache on the PXA250/210.
# It currently flushes the main dcache only.
#
# The line-allocate operation does not require physical memory to exist at the virtual address
#  specified by the instruction, since it does not generate a load/fill request to external memory. Also,
#  the line-allocate operation does not set the 32 bytes of data associated with the line to any known
#  value. Reading this data produces unpredictable results.
#
# *NOTE:  This routine WILL BE PREEMPTED!
#
# **NOTE:  This routine operates in Virtual Space.
#
# Syntax:
#   void FlushDCache(void)#
#
# Arguments:
#   -- none --
#
# Return Value:
#   -- none --
# r0...r1:  junk
# CC flags: junk
#--

# *****
# Global Clean/Invalidate THE DATA CACHE
# R1 contains the virtual address of a non-existent region of cacheable memory.
# R0 is the loop count# Currently doing 4 allocations per iteration (256 iterations)
#
#*** Note:  Currently, we are NOT cleaning the mini-data cache.  IF you intend on using
#   this feature, you must include a flushing routine as per the XScale core EAS:
#
     #
     # Clean the Mini-data Cache
     # Can't use line-allocate command, so cycle through 2KB of unused data.
     # R2 contains the virtual address of a region of cacheable memory mapped to the mini-D, reserved for
     # cleaning the Mini-data Cache (in config.bib).
     # R0 is the loop count# Iterate 64 times which is the number of lines in the
     # Mini-data Cache.

        #mov r0, #64
     #LOOP2
        #ldr  r3, [r2], #+32 # Load and increment to next cache line
        #subs r0, r0, #1    // Decrement loop count
        #BNE  LOOP2
###############################################################################

#define CACHE_FLG               0
#define ZBANK_BASE_C_VIRTUAL    0xffe00000  /* cached virtual */
#define DCACHE_CACHELINECOUNT   256     /* 256*4=1024 */

        #
        #   Check global flag to see which 32K buffer to use. We will alternate from call to call.
        #     0: 0->32K
        #     1: 32K->64K
        #
#        ldr     r2, =CACHE_FLG                          // cached flag buffer
        ldr     r0, =ZBANK_BASE_C_VIRTUAL               // use lower range
        mov     R1, #DCACHE_CACHELINECOUNT
#        ldr     r3, [r2]
#        tst     r3, #1
#        addne   r0, r0, #0x8000                         // use upper range
#        eor     r3, r3, #1
#        str     r3, [r2]                                // set/clear flag


        # ** DCache Cleanser **
LOOP1:
        mcr p15, 0, r0, c7, c2, 5               // Allocate a line at the virtual address ZBANK_BASE_C_VIRTUAL
                                                //  (tossing out a dirty line back to memory)
        add r0, r0, #+32
        mcr p15, 0, r0, c7, c2, 5

        add r0, r0, #+32
        mcr p15, 0, r0, c7, c2, 5

        add r0, r0, #+32
        mcr p15, 0, r0, c7, c2, 5

        ADD R0, R0, #+32                         // Increment the address in R0 to the next cache line
        SUBS R1, R1, #1                         // Decrement loop count
        BNE LOOP1


        # drain the Write and Fill buffers
        mcr p15, 0, r0, c7, c10, 4

    mov   pc, lr            // return


C_SYMBOL(pxa_cpu_standby12):
    stmfd   sp!, {r1 - r9, lr}
    mov r1,     #0x18
    mov r2,     #2
    ldr r3,     =0xe0F00004
    mov r7,     #0xff000000

    ldr r4, =0x88000004
    ldr r5, [r4]
    mov r6, r5
    ldr r4, =0xE3D5AFFF
    and r5, r5, r4
    ldr r4, =0x88000004
    str r5,[r4]

#   mov r5, #0x0ff
#   ldr r7, =0x0f4000010
#   str r5, [r7]

    mcr p14, 0, r2, c7, c0, 0

    str r6, [r4]
    str r1, [r3]
    ldmfd   sp!, {r1 - r9, pc}

.extern _get_sleep_phys_sp
.extern chat
.extern aDelay


C_SYMBOL(pxa_cpu_suspend):
#   mra r2, r3, acc0
    stmfd   sp!, {r2 - r12, lr}     // save registers on stack

#    bl chat

    # get coprocessor registers
    mrc p15, 0, r4, c15, c1, 0      // CP access reg
    mrc p15, 0, r5, c13, c0, 0      // PID
    mrc     p15, 0, r6, c3, c0, 0       // domain ID
    mrc     p15, 0, r7, c2, c0, 0       // translation table base addr
    mrc p15, 0, r8, c1, c1, 0           // auxiliary control reg
    mrc     p15, 0, r9, c1, c0, 0       // control reg

    # store them plus current virtual stack ptr on stack
    mov r10, sp
    stmfd   sp!, {r4 - r10}

    # preserve phys address of stack
    mov r0, sp

    bl  _get_sleep_phys_sp
    ldr r1, =sleep_save_sp
    str r0, [r1]

    # clean data cache
    #bl cpu_arm720_cache_clean_invalidate_all
    bl FlushDCache
    bl FlushICache

    # Put the processor to sleep
    # (also workaround for sighting 28071)

    #bl _chat

    # prepare value for sleep mode
    mov r1, #3              // sleep mode

    # prepare to put SDRAM into self-refresh manually
    ldr r4, =0x88000004
    ldr r5, [r4]
    orr r5, r5, #(1<<22)        //MDREFR_SLFRSH

    # prepare pointer to physical address 0 (virtual mapping in generic.c)
#   mov r2, #UNCACHED_PHYS_0

    # add for sightings.
#   ldr r4, =0xE3D5AFFF
#   and r5, r5, r4
#   ldr r4, =0x88000004

#############    ldr r5, [r4]

#    ldr r3, =0x80E00118 #GPSR3
#    mov r2, #0x40000  //LED Red
#    str r2, [r3]
#
#    bl aDelay
#   # prepare value for sleep mode
#   mov r1, #3              // sleep mode
#
#    ldr r3, =0x80E00124 #GPCR3
#    mov r2, #0x40000  //LED Red
#    str r2, [r3]
#
    # put SDRAM into self-refresh
#    ldr r5, [r4]
    str r5, [r4]
    ldr r5, [r4]
#    bl _chat

    # force address lines low by reading at physical address 0
#   ldr r3, [r2]

    # force address lines low by reading at physical address 0
#   mov r2, #UNCACHED_PHYS_0
#   ldr r3, [r2]

#    b %f20

    # enter sleep mode
    mcr p14, 0, r1, c7, c0, 0

b20:
    NOP
    b   b20                // loop waiting for sleep


C_SYMBOL(pxa_cpu_resume):
#   b pxa_cpu_resume
    NOP
#    NOP
#    NOP
    NOP
    NOP

    NOP
    NOP
    NOP
    NOP
    NOP
    NOP

#    mov	r2, #0x38
#bitset1:
#      ldr    r0,  = 0x40700000
#      ldr    r1,  [r0, #0x14]	   @  Get Line Status Register Data
#      ands   r1,  r1,  #0x20               @  Is TDRQ (Transmit Data Request) bit set?
#      beq    bitset1	                   @  No - loop until it is
#      strb   r2,  [r0, #0]	   @  It's ready! - output byte to buffer

#    ldr r11, =0x40E00020
#	ldr r12, [r11]
#	orr r12, r12, #0x2000000 //for test
#	str r12, [r11]

#    bl chat

    mov r0, #0xd3 @I_BIT | F_BIT | MODE_SVC
    msr cpsr_c, r0
    ldr r0, sleep_save_sp

    ldr r2, =resume_after_mmu
    ldmia   r0, {r4 - r9, sp}

#    bl chat

11:
    mov r1, #0

    mcr p15, 0, r1, c8, c7, 0 //invalid i/d tlb
    mcr p15, 0, r1, c7, c7, 0 //invalid i/d cache and btb

    mcr p15, 0, r4, c15, c1, 0
    mcr p15, 0, r5, c13, c0, 0
    mcr     p15, 0, r6, c3, c0, 0

#    ldr r1, =0xa00f0000
#    cmp r1, r7
#    ldreq r3, =0x40E00124 #GPCR3
#    moveq r2, #0x40000  //LED Red off
#    streq r2, [r3]

    mcr     p15, 0, r7, c2, c0, 0
   NOP
   NOP
   NOP
   NOP
   NOP
   NOP
   NOP
   NOP
   NOP

#    ldr r3, =0x80E00124 #GPCR3
#    mov r2, #0x40000  //LED Red off
#    str r2, [r3]

    mcr p15, 0, r8, c1, c1, 0
    b   resume_turn_on_mmu

resume_turn_on_mmu:  //align with 0x10
    mcr p15, 0, r1, c10, c4, 1 // # unlock I-TLB
    mcr p15, 0, r1, c8, c5, 0 //# invalidate I-TLB
    mrc p15, 0, r1, c2, c0, 0 //# CPWAIT
    mov r1, r1
    sub pc, pc, #4
    b afteralign //# branch to aligned code

#    NOP
#    NOP
#    NOP
#    NOP
#    NOP
#    NOP
#    NOP
#    NOP
#    NOP
#    NOP

#    NOP
#    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP

    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP

    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP

    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP
    NOP

afteralign:  //align with 0x100

    mcr     p15, 0, r9, c1, c0, 0

    //ldr    r7,=0xe0E00124
    //ldr    r8,=0x001c0000
    //str    r8,[r7]
    //ldr    r7,=0xe0E00118
    //ldr    r8,=0x00080000
    //str    r8,[r7]

    mrc p15, 0, r1, c2, c0, 0
    sub pc, r2, r1, lsr #32 //?
    nop
    nop
    nop
    nop

C_SYMBOL(sleep_save_sp):
sleep_save_sp:
    .word 0

    .text
resume_after_mmu:

#    ldr r3, =0x80E00124 #GPCR3
#    mov r2, #0x40000  //LED Red off
#    str r2, [r3]

#   bl  cpu_xscale_proc_init
    ldmfd   sp!, {r2, r3}
#    mar acc0, r2, r3
    ldmfd   sp!, {r4 - r12, pc}

#===================================================================
    .macro GET_CLKCFG rd


        // read CLKCFG's value
        mrc p14, 0, \rd, c6, c0, 0


        // mask out irrelevant bits
        and     \rd, \rd, #0xF

    .endm

    .macro PUT_CLKCFG rd


        // mask out irrelevant bits
        and     \rd, \rd, #0xF

        // write CLKCFG's value
        mcr p14, 0, \rd, c6, c0, 0

    .endm


C_SYMBOL(ReadCLKCFG):
        GET_CLKCFG   r0
        mov pc, lr

C_SYMBOL(WriteCLKCFG):
        PUT_CLKCFG   r0
        mov pc, lr

C_SYMBOL(WritePWRMODE):
        mcr p14, 0, r0, c7, c0, 0
        mov pc, lr

C_SYMBOL(pmu_start_S):
    mcr p14, 0, r0, c8, c1, 0
    mcr p14, 0, r1, c4, c1, 0
    mcr p14, 0, r2, c0, c1, 0
    mov pc, lr

C_SYMBOL(ReadCCNT):
    mrc p14, 0, r0, c1, c1, 0
    mov pc, lr

C_SYMBOL(ReadPMN0):
    mrc p14, 0, r0, c0, c2, 0
    mov pc, lr

C_SYMBOL(ReadPMN1):
    mrc p14, 0, r0, c1, c2, 0
    mov pc, lr

C_SYMBOL(ReadPMN2):
    mrc p14, 0, r0, c2, c2, 0
    mov pc, lr

C_SYMBOL(ReadPMN3):
    mrc p14, 0, r0, c3, c2, 0
    mov pc, lr

C_SYMBOL(ClearPMNC):
    mcr p14, 0, r0, c0, c1, 0
    mov pc, lr

C_SYMBOL(set_cpu_mode):
    mcr  p14, 0, r0, c6, c0, 0
    mov pc, lr

C_SYMBOL(get_cpu_mode):
    mrc  p14, 0, r0, c6, c0, 0
    mov pc, lr

C_SYMBOL(GetSP):
    mov  r0, sp
    mov  pc, lr

#define TTBMASK         0xffffc000
C_SYMBOL(GetTTB):
        MRC     p15, 0, r0, c2, c0, 0
        NOP
        NOP
        LDR     r1, =TTBMASK
        AND     r0, r0, r1
        MOV     PC, LR
